#include "Astronomy/Constant.hpp"
#include "Astronomy/Parallax.hpp"
#include "Astronomy/Coordinate.hpp"
#include "Astronomy/Transformation.hpp"

#include <cmath>

Coordinate3D Parallax::Correction(Coordinate3D &coord, const double H, const double GeographicalLatitude, const double Height)
{
    double dw = 1;
    if(coord.Z < 500)
        dw = Constants::AU;
    coord.Z *= dw;
    const double U { atan(Constants::WGS84_CONSTANT * tan(GeographicalLatitude))};
    const double G {coord.X + H};

    //站点与地地心向径的赤道投影长度;
    double r0 = Constants::EarthRadius * cos(U) + Height * cos(GeographicalLatitude);
    //站点与地地心向径的轴向投影长度;
    double z0 = Constants::EarthRadius * sin(U) * Constants::WGS84_CONSTANT + Height * sin(GeographicalLatitude);
    double x0 = r0 * cos(G);
    double y0 = r0 * sin(G);

    // 球面坐标转直角坐标;
    auto s = Transformation::SphericalToCartesian(coord);
    // 减去向量;
    s.X -= x0;
    s.Y -= y0;
    s.Z -= z0;

    // 直角坐标转球面坐标;
    s = Transformation::CartesianToSpherical(s);
    //还原;
    s.Z /= dw;
    return s;;
}